// -----------------------------
// Author: 		Dhaenens Gert	
// Creation: 	12/29/2013 9:42:29 PM				
// -----------------------------

#include "Matrix.h"
#include "CMath.h"

/* Constructors */
Matrix::Matrix()
	: x(1.0f, 0.0f, 0.0f, 0.0f)
	, y(0.0f, 1.0f, 0.0f, 0.0f)
	, z(0.0f, 0.0f, 1.0f, 0.0f)
	, t(0.0f, 0.0f, 0.0f, 1.0f)
{
}

Matrix::Matrix(Vector4 x, Vector4 y, Vector4 z, Vector4 t)
	: x(x)
	, y(y)
	, z(z)
	, t(t)
{
}

Matrix::Matrix(Vector4 t)
	: t(t)
	, x(1.0f, 0.0f, 0.0f, 0.0f)
	, y(0.0f, 1.0f, 0.0f, 0.0f)
	, z(0.0f, 0.0f, 1.0f, 0.0f)
{
}

Matrix::Matrix(Float arr[16])
	: x(arr[0], arr[1], arr[2], arr[3])
	, y(arr[4], arr[5], arr[6], arr[7])
	, z(arr[8], arr[9], arr[10], arr[11])
	, t(arr[12], arr[13], arr[14], arr[15])
{
}

Matrix::Matrix(const Matrix& refMatrix)
	: x(refMatrix.x)
	, y(refMatrix.y)
	, z(refMatrix.z)
	, t(refMatrix.t)
{
}


Matrix& Matrix::operator=(const Matrix& refMatrix)
{
	x = refMatrix.x;
	y = refMatrix.y;
	z = refMatrix.z;
	t = refMatrix.t;
	return (*this);
}


/* Comparison Operators */
Bool Matrix::operator==(const Matrix& refMatrix) const
{
	return (x == refMatrix.x) &&
		(y == refMatrix.y) &&
		(z == refMatrix.z) &&
		(t == refMatrix.t);
}

Bool Matrix::operator!=(const Matrix& refMatrix) const
{
	return (x != refMatrix.x) ||
		(y != refMatrix.y) ||
		(z != refMatrix.z) ||
		(t != refMatrix.t);
}

Bool Matrix::operator<(const Matrix& refMatrix) const
{
	return (x < refMatrix.x) &&
		(y < refMatrix.y) &&
		(z < refMatrix.z) &&
		(t < refMatrix.t);
}

Bool Matrix::operator>(const Matrix& refMatrix) const
{
	return (x > refMatrix.x) &&
		(y > refMatrix.y) &&
		(z > refMatrix.z) &&
		(t > refMatrix.t);
}

Bool Matrix::operator<=(const Matrix& refMatrix) const
{
	return (x <= refMatrix.x) &&
		(y <= refMatrix.y) &&
		(z <= refMatrix.z) &&
		(t <= refMatrix.t);
}

Bool Matrix::operator>=(const Matrix& refMatrix) const
{
	return (x >= refMatrix.x) &&
		(y >= refMatrix.y) &&
		(z >= refMatrix.z) &&
		(t >= refMatrix.t);
}


/* Math In-Place Operators */
Matrix& Matrix::operator*=(const Matrix& refMatrix)
{
	(*this) = refMatrix.Transform((*this));
	return (*this);
}

Matrix& Matrix::operator/=(const Matrix& refMatrix)
{
	(*this) = refMatrix.Untransform((*this));
	return (*this);
}


/* Math Out-Of-Place Operators */
Matrix Matrix::operator*(const Matrix& refMatrix) const
{
	Matrix tmp(*this);
	tmp *= refMatrix;
	return tmp;
}

Matrix Matrix::operator/(const Matrix& refMatrix) const
{
	Matrix tmp(*this);
	tmp /= refMatrix;
	return tmp;
}


/* Math Friend Operators */
Vector4 operator*(const Vector4& refVector, const Matrix& refMatrix)
{
	return refMatrix.Transform(refVector);
}

Vector4 operator/(const Vector4& refVector, const Matrix& refMatrix)
{
	return refMatrix.Untransform(refVector);
}


/* Methods */
Vector4 Matrix::Transform(const Vector4& refVector) const
{
	Float m[16];
	Float v[4];
	Float o[4];

	this->AsArray(m);
	refVector.AsArray(v);

	o[0] = (v[0] * m[0]) + (v[1] * m[4]) + (v[2] * m[8]) + (v[3] * m[12]);
	o[1] = (v[0] * m[1]) + (v[1] * m[5]) + (v[2] * m[9]) + (v[3] * m[13]);
	o[2] = (v[0] * m[2]) + (v[1] * m[6]) + (v[2] * m[10]) + (v[3] * m[14]);
	o[3] = (v[0] * m[3]) + (v[1] * m[7]) + (v[2] * m[11]) + (v[3] * m[15]);

	return Vector4(o);
}

Matrix	Matrix::Transform(const Matrix& refMatrix) const
{
	Float ma[16];
	Float mb[16];
	Float o[16];

	refMatrix.AsArray(ma);
	this->AsArray(mb);

	o[0] = (ma[0] * mb[0]) + (ma[1] * mb[4]) + (ma[2] * mb[8]) + (ma[3] * mb[12]);
	o[1] = (ma[0] * mb[1]) + (ma[1] * mb[5]) + (ma[2] * mb[9]) + (ma[3] * mb[13]);
	o[2] = (ma[0] * mb[2]) + (ma[1] * mb[6]) + (ma[2] * mb[10]) + (ma[3] * mb[14]);
	o[3] = (ma[0] * mb[3]) + (ma[1] * mb[7]) + (ma[2] * mb[11]) + (ma[3] * mb[15]);

	o[4] = (ma[4] * mb[0]) + (ma[5] * mb[4]) + (ma[6] * mb[8]) + (ma[7] * mb[12]);
	o[5] = (ma[4] * mb[1]) + (ma[5] * mb[5]) + (ma[6] * mb[9]) + (ma[7] * mb[13]);
	o[6] = (ma[4] * mb[2]) + (ma[5] * mb[6]) + (ma[6] * mb[10]) + (ma[7] * mb[14]);
	o[7] = (ma[4] * mb[3]) + (ma[5] * mb[7]) + (ma[6] * mb[11]) + (ma[7] * mb[15]);

	o[8] = (ma[8] * mb[0]) + (ma[9] * mb[4]) + (ma[10] * mb[8]) + (ma[11] * mb[12]);
	o[9] = (ma[8] * mb[1]) + (ma[9] * mb[5]) + (ma[10] * mb[9]) + (ma[11] * mb[13]);
	o[10] = (ma[8] * mb[2]) + (ma[9] * mb[6]) + (ma[10] * mb[10]) + (ma[11] * mb[14]);
	o[11] = (ma[8] * mb[3]) + (ma[9] * mb[7]) + (ma[10] * mb[11]) + (ma[11] * mb[15]);

	o[12] = (ma[12] * mb[0]) + (ma[13] * mb[4]) + (ma[14] * mb[8]) + (ma[15] * mb[12]);
	o[13] = (ma[12] * mb[1]) + (ma[13] * mb[5]) + (ma[14] * mb[9]) + (ma[15] * mb[13]);
	o[14] = (ma[12] * mb[2]) + (ma[13] * mb[6]) + (ma[14] * mb[10]) + (ma[15] * mb[14]);
	o[15] = (ma[12] * mb[3]) + (ma[13] * mb[7]) + (ma[14] * mb[11]) + (ma[15] * mb[15]);

	return Matrix(o);
}

Vector4 Matrix::Untransform(const Vector4& refVector) const
{
	Matrix m = this->Inverse();
	return m.Transform(refVector);
}

Matrix	Matrix::Untransform(const Matrix& refMatrix) const
{
	Matrix m = this->Inverse();
	return m.Transform(refMatrix);
}

Matrix Matrix::Inverse() const
{
	Float m[16];
	Float o[16];

	Matrix adj = this->Adjugate();
	Float detInv = 1.0f / this->Determinant();

	adj.AsArray(m);

	for (UInt32 i = 0; i < 16; ++i)
		o[i] = m[i] * detInv;

	return Matrix(o);
}

Float Matrix::Minor(UInt32 i) const
{
	UInt32 x = i % 4;
	UInt32 y = i / 4;

	Float m[16];
	Float c[3][3];

	this->AsArray(m);

	// Remove the Row & Column
	UInt32 uOffset = 0;
	for (UInt32 j = 0; j < 16; ++j)
	{
		UInt32 ux = j % 4;
		UInt32 uy = j / 4;

		if (ux == x || uy == y)
		{
			++uOffset;
			continue;
		}
		UInt32 uIndex = (uOffset == 0) ? (j) : (j - uOffset);
		UInt32 nx = uIndex % 3;
		UInt32 ny = uIndex / 3;
		c[nx][ny] = m[j];
	}

	// Find the 3x3 Determinant
	return c[0][0] * ((c[1][1] * c[2][2]) - (c[2][1] * c[1][2])) -
		c[0][1] * ((c[1][0] * c[2][2]) - (c[2][0] * c[1][2])) +
		c[0][2] * ((c[1][0] * c[2][1]) - (c[2][0] * c[1][1]));
}
Float Matrix::CoFactor(UInt32 i) const
{
	UInt32 x = i % 4;
	UInt32 y = i / 4;

	Float fMinor = Minor(i);
	return Math::Pow(-1.0f, (Float)(x + y)) * fMinor;
}

Matrix Matrix::CoFactorMatrix() const
{
	Float o[16];

	for (UInt32 i = 0; i < 16; ++i)
		o[i] = this->CoFactor(i);

	return Matrix(o);
}

Matrix Matrix::Transpose() const
{
	Float m[16];
	Float o[16];

	this->AsArray(m);

	o[0] = m[0];
	o[1] = m[4];
	o[2] = m[8];
	o[3] = m[12];

	o[4] = m[1];
	o[5] = m[5];
	o[6] = m[9];
	o[7] = m[13];

	o[8] = m[2];
	o[9] = m[6];
	o[10] = m[10];
	o[11] = m[14];

	o[12] = m[3];
	o[13] = m[7];
	o[14] = m[11];
	o[15] = m[15];

	return Matrix(o);
}

Matrix Matrix::Adjugate() const
{
	return (this->CoFactorMatrix().Transpose());
}

Float Matrix::Determinant() const
{
	Float m[16];

	this->AsArray(m);

	return m[3] * m[6] * m[9] * m[12] - m[2] * m[7] * m[9] * m[12] -
		m[3] * m[5] * m[10] * m[12] + m[1] * m[7] * m[10] * m[12] +
		m[2] * m[5] * m[11] * m[12] - m[1] * m[6] * m[11] * m[12] -
		m[3] * m[6] * m[8] * m[13] + m[2] * m[7] * m[8] * m[13] +
		m[3] * m[4] * m[10] * m[13] - m[0] * m[7] * m[10] * m[13] -
		m[2] * m[4] * m[11] * m[13] + m[0] * m[6] * m[11] * m[13] +
		m[3] * m[5] * m[8] * m[14] - m[1] * m[7] * m[8] * m[14] -
		m[3] * m[4] * m[9] * m[14] + m[0] * m[7] * m[9] * m[14] +
		m[1] * m[4] * m[11] * m[14] - m[0] * m[5] * m[11] * m[14] -
		m[2] * m[5] * m[8] * m[15] + m[1] * m[6] * m[8] * m[15] +
		m[2] * m[4] * m[9] * m[15] - m[0] * m[6] * m[9] * m[15] -
		m[1] * m[4] * m[10] * m[15] + m[0] * m[5] * m[10] * m[15];
}

void Matrix::AsArray(Float arr[16]) const
{
	arr[0] = x.x;
	arr[1] = x.y;
	arr[2] = x.z;
	arr[3] = x.w;

	arr[4] = y.x;
	arr[5] = y.y;
	arr[6] = y.z;
	arr[7] = y.w;

	arr[8] = z.x;
	arr[9] = z.y;
	arr[10] = z.z;
	arr[11] = z.w;

	arr[12] = t.x;
	arr[13] = t.y;
	arr[14] = t.z;
	arr[15] = t.w;
}

Matrix Matrix::LookAt(const Vector4& vPosition, const Vector3& vDirection, const Vector3& vUp)
{
	Vector3 vEye = Vector3(vPosition);
	Vector3 vAxisZ = vDirection.Normalized();
	Vector3 vAxisX = vUp.CrossProduct(vAxisZ).Normalized();
	Vector3 vAxisY = vAxisZ.CrossProduct(vAxisX).Normalized();

	Matrix r = Matrix(Vector4(vAxisX.x, vAxisY.x, vAxisZ.x, 0.0f),
					  Vector4(vAxisX.y, vAxisY.y, vAxisZ.y, 0.0f),
					  Vector4(vAxisX.z, vAxisY.z, vAxisZ.z, 0.0f),
					  Vector4(vAxisX.DotProduct(-vEye), vAxisY.DotProduct(-vEye), vAxisZ.DotProduct(-vEye), 1.0f));

	//Matrix t = Matrix::Translation(-vPosition);

	return r;
}
Matrix Matrix::Perspective(Float fFov, Float fAspectRatio, Float fNear, Float fFar)
{
	Float yScale = 1.0f / Math::Tan(fFov / 2.0f);
	Float xScale = yScale / fAspectRatio;
	Float fRatio = fFar / (fFar - fNear);
	
	Matrix m = Matrix(Vector4(xScale, 0.0f, 0.0f, 0.0f),
					  Vector4(0.0f, yScale, 0.0f, 0.0f),
					  Vector4(0.0f, 0.0f, fFar / (fFar - fNear), 1.0f),
					  Vector4(0.0f, 0.0f, -fNear * fFar / (fFar - fNear), 0.0f));
	return m;
}

Matrix Matrix::RotationX(Float fAngle)
{
	Float s = Math::Sin(fAngle);
	Float c = Math::Cos(fAngle);
	Matrix m = Matrix(Vector4(1.0f, 0.0f, 0.0f, 0.0f),
		Vector4(0.0f, c, -s, 0.0f),
		Vector4(0.0f, s, c, 0.0f),
		Vector4(0.0f, 0.0f, 0.0f, 1.0f));
	return m;
}
Matrix Matrix::RotationY(Float fAngle)
{
	Float s = Math::Sin(fAngle);
	Float c = Math::Cos(fAngle);
	Matrix m = Matrix(Vector4(c, 0.0f, s, 0.0f),
		Vector4(0.0f, 1.0f, 0.0f, 0.0f),
		Vector4(-s, 0.0f, c, 0.0f),
		Vector4(0.0f, 0.0f, 0.0f, 1.0f));
	return m;
}
Matrix Matrix::RotationZ(Float fAngle)
{
	Float s = Math::Sin(fAngle);
	Float c = Math::Cos(fAngle);
	Matrix m = Matrix(Vector4(c, -s, 0.0f, 0.0f),
		Vector4(s, c, 0.0f, 0.0f),
		Vector4(0.0f, 0.0f, 1.0f, 0.0f),
		Vector4(0.0f, 0.0f, 0.0f, 1.0f));

	return m;
}
Matrix Matrix::RotationXYZ(Float fAngleX, Float fAngleY, Float fAngleZ)
{
	Matrix mX = RotationX(fAngleX);
	Matrix mY = RotationY(fAngleY);
	Matrix mZ = RotationZ(fAngleZ);

	return (mX*mY*mZ);
}
Matrix Matrix::RotateAround(Float fAngle, const Vector3& vAxis)
{
	Float c = Math::Cos(fAngle);
	Float s = Math::Sin(fAngle);
	Vector3 v = vAxis.Normalized();

	Vector4 x = Vector4(c + (v.x*v.x) * (1.0f - c),
					    v.x*v.y * (1.0f - c) - v.z * s,
					    v.x*v.z * (1.0f - c) + v.y * s,
					    0.0f);

	Vector4 y = Vector4(v.y*v.x * (1.0f - c) + v.z * s,
						c + (v.y*v.y) * (1.0f - c),
						v.y*v.z * (1.0f - c) - v.x * s,
						0.0f);

	Vector4 z = Vector4(v.z*v.x * (1.0f - c) - v.y * s,
						v.z*v.y * (1.0f - c) + v.x * s,
						c + (v.z*v.z) * (1.0f - c),
				    	0.0f);

	return Matrix(x,y,z, Vector4(0.0f,0.0f,0.0f,1.0f));
}
Matrix Matrix::RotateAround(Float fAngle, const Vector3& vAxis, const Vector4& vOrigin)
{
	return (Matrix::Translation(vOrigin) * Matrix::RotateAround(fAngle, vAxis) * Matrix::Translation(-vOrigin));
}
Matrix Matrix::Translation(const Vector4& vPosition)
{
	Matrix m;
	m.t = vPosition;
	return m;
}
Matrix Matrix::Translation(const Vector3& vPosition)
{
	Matrix m;
	m.t = Vector4(vPosition, 1.0f);
	return m;
}
Matrix Matrix::Scale(Float fScaleX, Float fScaleY, Float fScaleZ)
{
	Matrix m;
	m.x *= fScaleX;
	m.y *= fScaleY;
	m.z *= fScaleZ;
	return m;
}